#ifndef HELMETI2C_H
#define HELMETI2C_H

#include "HelmetGlobals.h"
#include "HelmetSystemConfig.h"
#include "HelmetUART.h"
#include "HelmetOutputs.h"
#include <stdio.h>

/**************************************************************************************
	Definitions
**************************************************************************************/
#define FuelGaugeClockFreq			100000
#define FuelGaugeSlaveAddr			0x34
#define FuelGaugeI2Cbus				I2C2
#define W /* write mode */			0x00
#define R /* read mode */			0x01

// Register Memory Locations
#define FUEL_GAUGE_STATUS_REG		0x01
#define FUEL_GAUGE_VOLT_REG			0x0C
#define FUEL_GAUGE_CMD_REG			0xFE
#define FUEL_GAUGE_RAAC_REG			0x02
#define FUEL_GAUGE_RARC_REG			0x06
#define FUEL_GAUGE_ACR_REG			0x10
#define FUEL_GAUGE_TEMP_REG			0x0A
#define FUEL_GAUGE_CURR_REG			0x0E

// Commandd Register Commands
#define FGCMD_RECALL_APP_PARAMETERS	0xB4 // takes 2ms
#define FGCMD_STORE_APP_PARAMETERS	0x44 // takes 2ms

// Application Parameters Memory Locations
#define FUEL_GAUGE_RSNSP_RAM		0x69
#define FUEL_GAUGE_VCHG_RAM			0x64
#define FUEL_GAUGE_VAE_RAM			0x66
#define FUEL_GAUGE_IMIN_RAM			0x65
#define FUEL_GAUGE_IAE_RAM			0x67
#define FUEL_GAUGE_AC_MSB_RAM		0x62
#define FUEL_GAUGE_AC_LSB_RAM		0x63
#define FUEL_GAUGE_AS_REG			0x14
#define FUEL_GAUGE_FULL40_MSB_RAM	0x6A
#define FUEL_GAUGE_FULL40_LSB_RAM	0x6B

// Application Parameter Dependences
#define FG_RSNSP					200 // Siemens ( 1 / <Rsns> )
#define FG_BATT_FULL				4400 // uVh ( <Batt's Ah> / RSNSP / 6.25uVh )
#define FG_R1						820 // kOhms (this connects Vin to Vbat)
#define FG_R2						200 // kOhms ( this connects Vin to ground)
#define FG_FULL_THRESH				16.5 // volts (this is the threshold for determining full status)
#define FG_EMPTY_THRESH				14.7 // volts (this is the threshold for determining empty status)

// Status Register Actions
#define ReadFuelGaugeStatusReg(x)	ReadFuelGaugeByte(FUEL_GAUGE_STATUS_REG,x)
#define ClearFuelGaugeStatusReg()	WriteFuelGaugeByte(0x00,FUEL_GAUGE_STATUS_REG)

// 
#define WriteFuelGaugeCommand(x)	WriteFuelGaugeByte(x,FUEL_GAUGE_CMD_REG)
#define FuelGaugeRecallAppParams()	WriteFuelGaugeCommand(FGCMD_RECALL_APP_PARAMETERS)
#define FuelGaugeStoreAppParams()	WriteFuelGaugeCommand(FGCMD_STORE_APP_PARAMETERS)
// Sets battery level to FG_BATT_FULL
#define SetBattLevelToFull()		WriteFuelGaugeByte( ((FG_BATT_FULL&0xFF00)>>8)&0x00FF, FUEL_GAUGE_ACR_REG ); \
									WriteFuelGaugeByte( FG_BATT_FULL&0x00FF, FUEL_GAUGE_ACR_REG+0x01 )
// Resets Full40 Register to 100%
#define ResetFull40()				WriteFuelGaugeByte( 0x40, FUEL_GAUGE_FULL40_MSB_RAM ); \
									WriteFuelGaugeByte( 0x0A, FUEL_GAUGE_FULL40_LSB_RAM )

/**************************************************************************************
	Global Variables
**************************************************************************************/

/**************************************************************************************
	Function Prototypes
**************************************************************************************/
void ConfigFuelGauge();
void DebugFuelGaugeSetup();
void OutputFuelGaugeData();
void VerifyFuelGaugeStatus();
unsigned short int FuelGaugeInitialSetup();
unsigned short int FuelGaugeSetAppParameter( int data, int caddr );
unsigned short int GetBatteryPercentRemaining( short int * percentRem );
unsigned short int GetBatteryCurrent( float * battCurrent );
unsigned short int GetBatteryTemp( float * battTemp );
unsigned short int GetBatteryCharge( float * mAhRemaining );
unsigned short int GetBatteryVoltage( float * Vbat );
unsigned short int GetFuelGaugeVinValue( float * Vin );
unsigned short int ReadFuelGaugeByte( int maddr /* mem addr to read */, int * data /* data to return */ );
unsigned short int WriteFuelGaugeByte( int data /* data to write */, int maddr /* address to write */ );
unsigned short int ReadFuelGaugeTwoBytes( int maddr /* address to read */, int * data /* data to return */ );

/**************************************************************************************
	Functions
**************************************************************************************/
void ConfigFuelGauge() {
	// Set the I2C baudrate
    UINT32 actualClock = I2CSetFrequency(FuelGaugeI2Cbus, GetPeripheralClock(), FuelGaugeClockFreq);
    if ( abs(actualClock-FuelGaugeClockFreq) > FuelGaugeClockFreq/10 ) {
        //WriteAtom("Error: I2C1 clock frequency error\r\n");
    }
    // Enable the I2C bus
    I2CEnable(FuelGaugeI2Cbus, TRUE);
    
    // perform initial set-up of fuel gauge (only needs to be done once)
    // unsigned int err = 9;
  	// err = FuelGaugeInitialSetup();
  	// if( err != 0 ) WriteAtom( "Error On Fuel Gauge Initial Set-up!\r\n" );
    // else WriteAtom( "Fuel Gauge Initial Set-up OKAY!\r\n" );
  	// ResetFull40();
  	// SetBattLevelToFull();
  	// DebugFuelGaugeSetup();
    
    // recall App Parameter EEPROM data into shadow RAM
    
    FuelGaugeRecallAppParams(); // disabled while 2782 is dead
}

void VerifyFuelGaugeStatus() {
	short int sidat = 55;
	unsigned short int err = 9;
	float data = 55;

	//char msg[50];
	
	// status
	err = ReadFuelGaugeStatusReg( (int*) &sidat );
	sidat = sidat & 0xF0;
	//sprintf( msg, "FGstt %.0f :%.0f\r\n", (float) sidat, (float) err );
	//WriteAtom( msg );
	if( (sidat&0x40) > 0 ) {
		WriteAtom("FGstt BATTERY EMPTY !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\r\n");
		SetErr2LED();
		DoInMain(EnterSleepMode);
	} else {
		WriteAtom("FGstt BATTERY GOOD\r\n");
	}	
}	

void OutputFuelGaugeData() {
	unsigned short int err = 9;
	float data = 55;
	short int sidat = 55;
	char msg[50];
	
	// status
	err = ReadFuelGaugeStatusReg( (int*) &sidat );
	sidat = sidat & 0xF0;
	sprintf( msg, "FGstt %.0f :%.0f\r\n", (float) sidat, (float) err );
	WriteAtom( msg );
	if( (sidat&0x40) > 0 ) {
		WriteAtom("FGstt BATTERY EMPTY !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\r\n");
		SetErr2LED();
		DoInMain(EnterSleepMode);
	} else if( (sidat&0x80) > 0 ) {
		WriteAtom("FGstt BATTERY FULL\r\n");
	} //else if( (sidat&0x06) > 0 ) {
		//WriteAtom("Clearing Status Reg\r\n");
		//ClearFuelGaugeStatusReg();
	//}
		
	// current
	err = GetBatteryCurrent( &data );
	sprintf( msg, "FGcnt %.5f A :%.0f\r\n", data, (float) err );
	WriteAtom( msg );
	
	// battery voltage
	err = GetBatteryVoltage( &data );
	sprintf( msg, "FGvlt %.2f V :%.0f\r\n", data, (float) err );
	WriteAtom( msg );
	
	// % remaining
	err = GetBatteryPercentRemaining( &sidat );
	sprintf( msg, "FGrel %.0f %% :%.0f\r\n", (float) sidat, (float) err );
	WriteAtom( msg );
	
	// abs charge
	err = GetBatteryCharge( &data );
	sprintf( msg, "FGabs %.1f mAh :%.0f\r\n", data, (float) err );
	WriteAtom( msg );
	
	// temp
	err = GetBatteryTemp( &data );
	sprintf( msg, "FGtmp %.1f *C :%.0f\r\n", data, (float) err );
	WriteAtom( msg );
//	if( data > 29.0 ) {
//		FansOn();
//	} else {
//		FansOff();
//	}	
	
}	

void DebugFuelGaugeSetup() {
	unsigned short int err = 9;
	float data = 55;
	short int sidat = 55;
	char msg[50];
	WriteAtom( "======== BEGIN FG DEBUG ======\r\n" );
	// RAAC
	err = GetBatteryCharge( &data );
	sprintf( msg, "%.0f : %.0f mAh remain\r\n", (float) err, data );
	WriteAtom( msg );
	// RSRC
	err = GetBatteryPercentRemaining( &sidat );
	sprintf( msg, "%.0f : %.0f %% remain\r\n", (float) err, (float) sidat );
	WriteAtom( msg );
	// RSNSP
	err = ReadFuelGaugeByte( FUEL_GAUGE_RSNSP_RAM, (int*) &sidat );
	sprintf( msg, "%.0f : %.0f RSNSP\r\n", (float) err, (float) sidat );
	WriteAtom( msg );
	// VCHG
	err = ReadFuelGaugeByte( FUEL_GAUGE_VCHG_RAM, (int*) &sidat );
	sprintf( msg, "%.0f : %.0f VCHG\r\n", (float) err, (float) sidat );
	WriteAtom( msg );
	// VAE
	err = ReadFuelGaugeByte( FUEL_GAUGE_VAE_RAM, (int*) &sidat );
	sprintf( msg, "%.0f : %.0f VAE\r\n", (float) err, (float) sidat );
	WriteAtom( msg );
	// IMIN
	err = ReadFuelGaugeByte( FUEL_GAUGE_IMIN_RAM, (int*) &sidat );
	sprintf( msg, "%.0f : %.0f IMIN\r\n", (float) err, (float) sidat );
	WriteAtom( msg );
	// IAE
	err = ReadFuelGaugeByte( FUEL_GAUGE_IAE_RAM, (int*) &sidat );
	sprintf( msg, "%.0f : %.0f IAE\r\n", (float) err, (float) sidat );
	WriteAtom( msg );
	// AC UB
	err = ReadFuelGaugeByte( FUEL_GAUGE_AC_MSB_RAM, (int*) &sidat );
	short int t = sidat << 8;
	// AC LB
	err = ReadFuelGaugeByte( FUEL_GAUGE_AC_LSB_RAM, (int*) &sidat );
	sidat = sidat + t;
	sprintf( msg, "%.0f : %.0f AC\r\n", (float) err, (float) sidat );
	WriteAtom( msg );
	// AS
	err = ReadFuelGaugeByte( FUEL_GAUGE_AS_REG, (int*) &sidat );
	sprintf( msg, "%.0f : %.0f AS\r\n", (float) err, (float) sidat );
	WriteAtom( msg );
	WriteAtom( "======== END FG DEBUG ========\r\n" );
}	

unsigned short int FuelGaugeInitialSetup() {
	unsigned short int err = 9;
	// Write RSNSP (sense resistor prime) value
    err = FuelGaugeSetAppParameter( (short int) FG_RSNSP, FUEL_GAUGE_RSNSP_RAM );
    // VCHG (charge voltage) value
    short int vchg = ( (float) FG_FULL_THRESH ) * ( (float) FG_R2 / (float) ( FG_R1 + FG_R2 ) ) / (float) 0.01952;
    err += FuelGaugeSetAppParameter( (short int) vchg, FUEL_GAUGE_VCHG_RAM );
    // VAE (active empty voltage) value
    short int vae = ( (float) FG_EMPTY_THRESH ) * ( (float) FG_R2 / (float) ( FG_R1 + FG_R2 ) ) / (float) 0.01952 + 1;
    err += FuelGaugeSetAppParameter( (short int) vae, FUEL_GAUGE_VAE_RAM );
    // IMIN (minimum charge current) value
    err += FuelGaugeSetAppParameter( (short int) 30, FUEL_GAUGE_IMIN_RAM );
    // IAE (active empty current) value
    err += FuelGaugeSetAppParameter( (unsigned short int) 1, FUEL_GAUGE_IAE_RAM );
    // AC (aging capacity) MSB, LSB
    err += FuelGaugeSetAppParameter( (FG_BATT_FULL&0xFF00)>>8, FUEL_GAUGE_AC_MSB_RAM );
    err += FuelGaugeSetAppParameter( (FG_BATT_FULL&0x00FF), FUEL_GAUGE_AC_LSB_RAM );
    // AS (age scalar) ram value
    err += WriteFuelGaugeByte( (short int) 122, FUEL_GAUGE_AS_REG ); // 122 ~= 95%
    return( err );
}	

unsigned short int FuelGaugeSetAppParameter( int data, int caddr ) {
	unsigned short int err = 9;
	// write data to shadow RAM
	err = WriteFuelGaugeByte( data, caddr );
	// copy shadow RAM to EEPROM
	FuelGaugeStoreAppParams();
	// wait >2ms
	int i=300000; // i-nominal is 160000
	while( i>0 ) i--;
	return( err );
}	

unsigned short int GetBatteryPercentRemaining( short int * percentRem ) {
	int PCT = 0;
	unsigned short int err = 9;
	err = ReadFuelGaugeByte( FUEL_GAUGE_RARC_REG, &PCT );
	if( err != 0 ) { return( err ); }
	// convert to percent
	*percentRem = (short int)PCT; // resolution is 1%
	return( 0 );	
}	

unsigned short int GetBatteryCurrent( float * battCurrent ) {
	int CURR = 0;
	unsigned short int err = 9;
	err = ReadFuelGaugeTwoBytes( FUEL_GAUGE_CURR_REG, &CURR ); // CURRENT
	if( err != 0 ) { return( err ); }
	// convert to amps
	*battCurrent = ((short int)CURR)* .0000015625 * FG_RSNSP; // resolution is 1.5625uV/Rsns OR 1.5635uV*RSNSP
	return( 0 );
}	

unsigned short int GetBatteryTemp( float * battTemp ) {
	int TEMP = 0;
	unsigned short int err = 9;
	err = ReadFuelGaugeTwoBytes( FUEL_GAUGE_TEMP_REG, &TEMP );
	if( err != 0 ) { return( err ); }
	// convert to degrees Celsius
	*battTemp = ((short int)TEMP)/32*0.125; // resolution is .125 *C
	return( 0 );
}	

unsigned short int GetBatteryCharge( float * mAhRemaining ) {
	int RAAC = 0;
	unsigned short int err = 9;
	err = ReadFuelGaugeTwoBytes( FUEL_GAUGE_RAAC_REG, &RAAC); // RAAC
	if( err != 0 ) { return( err ); }
	// convert to mAh
	*mAhRemaining = RAAC*1.6; // resolution is 1.6 mAh
	return( 0 );
}	

unsigned short int GetBatteryVoltage( float * Vbat /* converted battery voltage data output */ ) {
	unsigned short int err = 9;
	float Vin;
	err = GetFuelGaugeVinValue( &Vin );
	if( err != 0 ) { return( err ); }
	// convert to battery voltage (fxn of R1 & R2)
	*Vbat = Vin * ( FG_R1 + FG_R2 ) / FG_R2;
	return( 0 );
}	

unsigned short int GetFuelGaugeVinValue( float * Vin /* raw voltage data output */ ) {
	unsigned short int err = 9;
	int vdat = 0; // 
	err = ReadFuelGaugeTwoBytes( FUEL_GAUGE_VOLT_REG, &vdat );
	if( err != 0 ) { return( err ); }
	// convert to volts
	*Vin = ( (short int)vdat )/32*0.00488; // resolution is 4.88 mV
	return( 0 );
}	

unsigned short int ReadFuelGaugeByte( int maddr /* mem addr to read */, int * data /* data to return */ ) {
	// data to return
	//*data = 0;
	
	// make sure I2C2 is ready
	while( !I2CTransmitterIsReady( FuelGaugeI2Cbus ) );
	
	// READ FORMAT TEMPLATE FOR FUEL GAUGE (SEE DS2782 DATASHEET PG 25-26)
	// S		: Send start condition
	StartI2C2(); IdleI2C2();
	// SAddr W	: Write slave address in write mode
	MasterWriteI2C2( FuelGaugeSlaveAddr << 1 | W ); IdleI2C2();
	// [A]		: return 1 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2(); return( 1 );	}	
	// MAddr	: write memory address to fg
	MasterWriteI2C2( maddr ); IdleI2C2();
	// [A]		: return 2 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2(); return( 2 ); }	
	// Sr		: Send repeat start condition
	RestartI2C2(); IdleI2C2();
	// SAddr	: Write slave address in read mode
	MasterWriteI2C2( FuelGaugeSlaveAddr << 1 | R ); IdleI2C2();
	// [A]		: return 3 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2(); return( 3 ); }	
	// [Data0]	: Read in data value only if no errors
	* data = MasterReadI2C2();
	// N		: no ack from master
	NotAckI2C2();
	// P		: generate Stop bit
	StopI2C2(); IdleI2C2();
	
	// return 0 for no error
	return 0;
}

unsigned short int WriteFuelGaugeByte( int data /* data to write */, int maddr /* address to write */ ) {
	// error value
	unsigned short int rv=0;
	// make sure I2C2 is ready
	while( !I2CTransmitterIsReady( FuelGaugeI2Cbus ) );
	
	// WRITE FORMAT TEMPLATE FOR FUEL GAUGE (SEE DS2782 DATASHEET PG 25-26)
	// S		: Send start condition
	StartI2C2(); IdleI2C2();
	// SAddr W	: Write slave address in write mode
	MasterWriteI2C2( (FuelGaugeSlaveAddr<<1) | W ); IdleI2C2();
	// [A]		: return 1 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2(); return( 1 ); }
	// MAddr	: write memory address to fg
	MasterWriteI2C2( maddr ); IdleI2C2();
	// [A]		: return -1 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2(); return( 2 ); }
	// Data0	: write data to fg
	MasterWriteI2C2( data ); IdleI2C2();
	// [A]		: return -1 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2(); return( 3 ); }
	// P		: generate Stop bit
	StopI2C2(); IdleI2C2();
	
	// data written successfully
	return 0;
}

unsigned short int ReadFuelGaugeTwoBytes( int maddr /* address to read */, int * data /* data to return */ ) {
	// data to return
	//data = 0;
	
	// make sure I2C2 is ready
	while( !I2CTransmitterIsReady( FuelGaugeI2Cbus ) );
	
	// READ FORMAT TEMPLATE FOR FUEL GAUGE (SEE DS2782 DATASHEET PG 25-26)
	// S		: Send start condition
	StartI2C2(); IdleI2C2();
	// SAddr W	: Write slave address in write mode
	MasterWriteI2C2( FuelGaugeSlaveAddr << 1 | W ); IdleI2C2();
	// [A]		: return -1 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2(); return( 1 ); }	
	// MAddr	: write memory address to fg
	MasterWriteI2C2( maddr ); IdleI2C2();
	// [A]		: return -1 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2();  return( 2 ); }	
	// Sr		: Send repeat start condition
	RestartI2C2(); IdleI2C2();
	// SAddr	: Write slave address in read mode
	MasterWriteI2C2( FuelGaugeSlaveAddr << 1 | R ); IdleI2C2();
	// [A]		: return -1 if no ack from fg
	if( !I2CByteWasAcknowledged( FuelGaugeI2Cbus ) ) { StopI2C2(); IdleI2C2(); return( 3 ); }
	// [Data0]	: Read in data byte 1 only if no errors
	*data = 0;
	*data = MasterReadI2C2() << 8;
	// A		: ack from master
	AckI2C2(); IdleI2C2();
	// [Data1]	: read in data byte 2 only if no errors
	*data += MasterReadI2C2();
	// N		: no ack from master
	NotAckI2C2();	
	// P		: generate Stop bit
	StopI2C2(); IdleI2C2();
	
	// return read success
	return 0;
}	

/**************************************************************************************
	Interrupt handling routines
**************************************************************************************/
// Note: Priority 1 (lowest) to 7 (highest)

#endif